# -*- -*- -*- -*- -*- -*- -*-
# @Author  : mengzhe
# @Time    : 2025/1/8 11:49
# @File    : utils.py.py
# @function: 通用工具模块
# @input   :
# @output  :
# @Software: PyCharm 
# -*- -*- -*- -*- -*- -*- -*-
import struct


import crcmod.predefined
import pyproj
def format_response(status, message):
    return f"{status}:{message}"


# 数据转换类
def format_serial(serial_number):
    """
    格式化序列号，每两个字符之间加一个空格。

    :param serial_number: 序列号字符串，例如 'af3040336a45bd65'
    :return: 格式化后的字符串，例如 'af 30 40 33 6a 45 bd 65'
    """
    return ' '.join([serial_number[i:i+2] for i in range(0, len(serial_number), 2)])


def float16_to_float16_hex(float16):
    """
    将 浮点数转换为 2 字节 float16，并以小端十六进制字符串表示。

    :param float16: float，CPU 使用率，例如 17.3
    :return: str，小端模式的十六进制格式，例如 "48 88"
    """
    # 将浮点数转换为 2 字节的 float16 格式（小端默认）
    float16_bytes = struct.pack('<e', float16)
    # 转换为十六进制字符串，每个字节用 2 个字符表示
    hex_string = ' '.join(f'{byte:02x}' for byte in float16_bytes)
    return hex_string


def float32_to_float32_hex(float32):
    """
    将浮点数转换为 4 字节 float32，并以小端十六进制字符串表示。

    :param float32: float，要转换的浮点数，例如 17.3
    :return: str，小端模式的十六进制格式，例如 "cd cc 8a 41"
    """
    # 将浮点数转换为 4 字节的 float32 格式（小端默认）
    float32_bytes = struct.pack('<f', float32)
    # 转换为十六进制字符串，每个字节用 2 个字符表示
    hex_string = ' '.join(f'{byte:02x}' for byte in float32_bytes)
    return hex_string


def int16_to_int16_hex(value):
    """
    将整数转换为小端模式的 int16 并返回十六进制字符串表示。

    :param value: int，要转换的整数。
    :return: str，小端模式的 int16 十六进制字符串。
    """
    # 确保值在 int16 范围内
    if not (-2**15 <= value < 2**15):
        raise ValueError("值超出 int16 范围")

    # 转换为小端模式的 int16
    int16_bytes = struct.pack('<h', value)

    # 转换为十六进制字符串
    hex_string = ' '.join(f'{byte:02x}' for byte in int16_bytes)
    return hex_string

def int32_to_int32_hex(value):
    """
    将整数转换为小端模式的 int32 并返回十六进制字符串表示。

    :param value: int，要转换的整数。
    :return: str，小端模式的 int32 十六进制字符串。
    """
    # 确保值在 int32 范围内
    if not (-2**31 <= value < 2**31):
        raise ValueError("值超出 int32 范围")

    # 转换为小端模式的 int32
    int32_bytes = struct.pack('<i', value)

    # 转换为十六进制字符串
    hex_string = ' '.join(f'{byte:02x}' for byte in int32_bytes)
    return hex_string

def str_to_hex(s):
    """
    将字符串转换为小端模式的字节序列，并返回十六进制字符串表示。

    :param s: str，要转换的字符串。
    :return: str，小端模式的字节序列的十六进制字符串。
    """
    # 将字符串编码为字节序列
    str_bytes = s.encode('utf-8')

    # 转换为十六进制字符串
    hex_string = ' '.join(f'{byte:02x}' for byte in str_bytes)
    return hex_string


def ip_to_hex_string(ip):
    """
    将 IP 地址转换为以空格分隔的十六进制字符串（大写）。

    :param ip: str，IP 地址（例如 '192.168.1.1'）。
    :return: str，以空格分隔的十六进制字符串。
    """
    # 将 IP 地址拆分为四个部分并转换为整数
    parts = ip.split('.')

    # 将每个部分转换为十六进制，并拼接成字符串
    hex_parts = [f"{int(part):02X}" for part in parts]

    # 使用空格连接每个十六进制部分
    return ' '.join(hex_parts)


def mac_to_hex_string(mac):
    """
    将 MAC 地址转换为以空格分隔的十六进制字符串（大写）。

    :param mac: str，MAC 地址（例如 '0C:11:7F:03:8F:82'）。
    :return: str，以空格分隔的十六进制字符串。
    """
    # 将 MAC 地址拆分为多个部分并去掉冒号
    parts = mac.split(':')

    # 使用空格连接每个部分
    return ' '.join(parts)


def parse_key_value_pairs(output):
    """
    解析包含键值对的字符串输出，并将其存储为字典。

    :param output: str，包含键值对的字符串（例如 'key=value' 每行一个键值对）。
    :return: dict，解析后的键值对字典。
    """
    results = {}
    for line in output.splitlines():
        if '=' in line:
            key, value = line.split('=', 1)  # 分割键值对
            results[key.strip()] = value.strip()  # 去除空白并存储
    return results


    # 将长度不足 24 的部分用 '*' 补齐，确保输出格式每个字节间用空格隔开
def format_to_24_bytes(data):
    """
    将给定的数据格式化为24字节的字符串。

    小于24字节，将使用 '*' 进行右填充。大于24字节，将截断至24字节。将每个字节用空格分隔开。

    :param data: 需要格式化的数据。
    :return   格式化后的24字节字符串。  
    """
    # 将数据用 '*' 补齐到 24 字节
    padded_data = data.ljust(24, 'F')[:24]
    # 每两个字节为一组输出
    return ' '.join(padded_data[i:i + 2] for i in range(0, len(padded_data), 2))

def process_addresses(data):
    """
    处理返回值，统计地址个数，并提取地址部分。

    :param data: str，返回值字符串，例如 '192.168.1.101-64:40:30:af:bd:45;192.168.1.144-36:c9:e3:f1:b8:05;...'
    :return: tuple(int, list)，地址个数和提取的地址列表。
    """
    if not data:
        return 0, []

    # 按照分号分割数据
    entries = data.split(';')

    # 提取每个条目中 '-' 前的部分（即地址）
    addresses = [entry.split('-')[0] for entry in entries if '-' in entry]

    # 统计地址个数
    count = len(addresses)

    return count, addresses



def calculate_crc(data):
    # CRC校验
    # 使用 CRC-16/MODBUS 算法
    crc16 = crcmod.predefined.Crc('modbus')
    crc16.update(data)
    return crc16.crcValue


def calculate_crc_2_byte(data: bytes) -> bytes:
    """计算 CRC-16/MODBUS 并返回小端序字节"""
    data_bytes = data

    # 计算 CRC
    crc16 = crcmod.predefined.Crc('modbus')
    crc16.update(data_bytes)
    crc_value = crc16.crcValue

    # 转换为小端序字节并格式化为十六进制字符串
    low_byte = crc_value & 0xFF
    high_byte = (crc_value >> 8) & 0xFF
    return bytes([low_byte, high_byte])  # 返回类型为 bytes # 低字节在前，高字节在后


# def command_processor(command: bytes, result: str) -> str:
#     # 调用 modify_command 处理 command
#     frame_header = modify_command(command)
#
#     # 计算 result_length
#     result_length = int_to_little_endian_hex(int((len(result) + 1) / 3))
#
#
#
#     crc16=calculate_crc_2_byte(result)
#
#     # 修改执行状态
#     frame_header=modify_execution(frame_header,result)
#
#
#     if result=='FF FF FF FF':
#         print(f"指令执行异常: {frame_header} 00 00 00 00")
#         return frame_header + ' ' + '00 00 00 00'
#     if isinstance(result,str):
#         if result.split(':')[0]=='OK':
#             print(f"指令执行正常,无返回数据: {frame_header} 00 00 00 00")
#             return frame_header + ' ' + '00 00 00 00'
#         elif result.split(':')[0]=='ERROR':
#             # 修改执行状态
#             frame_header = modify_execution(frame_header,result)
#             print(f"指令执行异常: {frame_header} 00 00 00 00")
#             return frame_header + ' ' + '00 00 00 00'
#         else:
#             print(f"指令执行正常，有返回数据: {frame_header} {result_length} {crc16} {result[:200]}")
#             return frame_header + ' ' + result_length + ' ' + crc16 +' '+ result
#     elif isinstance(result,bytes):
#         result_length=int_to_little_endian_hex(len(result))
#         print(f"指令执行正常，有返回数据: {frame_header} {result_length} {result}")
#         return frame_header + ' ' + result_length + ' ' + (' '.join([f"{byte:02X}" for byte in result])[:200])
#     else:
#         return frame_header + ' ' + '00 00 00 00'

def modify_sub_frame_command(command, subframe_count):
    # 转换为可变的 bytearray
    cmd_array = bytearray(command)

    # 修改指令子帧号（使用循环变量i）

    cmd_array[14] = subframe_count  # 每次循环都会覆盖前一次的值，最终只保留最后一个i的值

    # 转回不可变的 bytes 类型
    new_command = bytes(cmd_array)

    return new_command[0:32]

def command_processor(command: bytes, result: str) -> str:
    # 调用 modify_command 处理 command
    base_frame_header = modify_command(command)

    # 计算原始 result_length
    original_result_length = len(result)

    if original_result_length > (1024*16):
        # 需要分段处理
        segments = []
        subframe_count = (original_result_length+(1024*16)-1)//(1024*16)  # 向上取整计算子帧数量

        for i in range(subframe_count):
            # 计算当前子帧的起始和结束位置
            start = i * (1024*16)
            end = min((i + 1) * (1024*16), len(result))
            sub_result = result[start:end]

            # 计算当前子帧的长度
            sub_result_length = len(sub_result).to_bytes(4, byteorder='little')
            # 计算当前子帧的 CRC
            sub_crc16 = calculate_crc_2_byte(sub_result)

            # 修改子帧号
            sub_frame_header = modify_sub_frame_command(base_frame_header,i)

            # 修改执行状态（每个子帧都需要修改）
            sub_frame_header = modify_execution(sub_frame_header, sub_result)

            # 构建子帧结果
            if len(sub_result) == 0:
                segments.append(sub_frame_header+b'0x000x000x000x00')
            else:
                segments.append(sub_frame_header+sub_result_length+sub_crc16+sub_result)

        # 拼接所有子帧结果                                                                                    zz
        final_result = segments
        print(f"指令执行正常，返回数据被分割为 {subframe_count} 个子帧")
        return final_result

    else:
        # 原始处理逻辑（保持不变）
        result_length = int_to_little_endian_hex(original_result_length)

        frame_header = modify_execution(base_frame_header, result)

        if result == 'FF FF FF FF':
            print(f"指令执行异常: {frame_header} 00 00 00 00")
            return frame_header

        if isinstance(result, str):
            if result.split(':')[0] == 'OK':
                print(f"指令执行正常,无返回数据: {frame_header} 00 00 00 00")
                return frame_header
                # return frame_header + ' ' + '00 00 00 00'
            elif result.split(':')[0] == 'ERROR':
                print(f"指令执行异常: {frame_header} 00 00 00 00")
                return frame_header
                # return frame_header + ' ' + '00 00 00 00'
            else:
                crc16 = calculate_crc_2_byte(result)
                print(f"指令执行正常，有返回数据: {frame_header} {result_length} {crc16} {result[:200]}")
                return frame_header + ' ' + result_length + ' ' + crc16 + ' ' + result

        elif isinstance(result, bytes):
            result_length = len(result).to_bytes(4, byteorder='little')
            crc16 = calculate_crc_2_byte(result)
            print(f"指令执行正常，有返回数据: {frame_header} {result_length} {crc16} {result}")
            return frame_header+result_length+crc16+result

        else:
            return frame_header + ' ' + '00 00 00 00'




def int_to_little_endian_hex(num, byte_size=4):
    # 计算返回数据长度
    """
    将整数转换为指定字节大小的小端模式十六进制字符串，且字符串中每两个字符用空格分隔。
    :param num: 要转换的整数
    :param byte_size: 字节大小，默认为 4 字节
    :return: 格式化后的小端模式十六进制字符串
    """
    # 将整数转换为指定字节大小的小端模式字节序列
    little_endian_bytes = num.to_bytes(byte_size, byteorder='little')
    # 将小端模式的字节序列转换为十六进制字符串
    little_endian_hex_str = little_endian_bytes.hex()
    # 插入空格，使其符合 40 06 00 00 格式
    formatted_hex_str = ' '.join([little_endian_hex_str[i:i+2] for i in range(0, len(little_endian_hex_str), 2)])
    return formatted_hex_str

def bytes_2_str_bytes(command):
    """
    从客户端套接字接收数据，并将其转换为十六进制字符串，再编码为字节对象。

    :param command: 接收的bytes数据
    :return: 处理后的字节对象
    """
    command_hex = ' '.join([f"{byte:02X}" for byte in command])
    command = command_hex.encode()
    return command

def str_bytes_2_bytes(command):
    """
    从客户端套接字接收数据，并将其转换为十六进制字符串，再编码为字节对象。

    :param command: 接收的bytes数据
    :return: 处理后的字节对象
    """
    # # 将字节对象转换为十六进制字符串（大写，不带空格）
    # command_hex = ' '.join([f"{byte:02X}" for byte in command])
    # 移除空格
    command_hex = command.replace(" ", "")

    # 将十六进制字符串转换回字节对象
    command_bytes = bytes.fromhex(command_hex)
    return command_bytes


def modify_command(command):


    # 转换为可变的 bytearray
    cmd_array = bytearray(command)

    # 直接修改前两个字节
    # 修改帧头
    cmd_array[0] = 0x33
    cmd_array[1] = 0xCC

    # 修改执行状态
    cmd_array[9] = 0x01

    # 修改指令子帧号
    cmd_array[14] = 0x00

    # 转回不可变的 bytes 类型
    new_command = bytes(cmd_array)

    return new_command[0:32]




def modify_execution(command,result):
    # 修改指令的执行状态

    # 转换为可变的 bytearray
    cmd_array = bytearray(command)

    if isinstance(result,str):
        if result.split(':')[0]=='OK':
            cmd_array[9] = 0x00   # 执行正常
        elif result.split(':')[0]=='ERROR':
            cmd_array[9] = 0x01   # 参数错误
        else:
            cmd_array[9] = 0x00   #其他返回值，执行正常
    elif isinstance(result,bytes):
        cmd_array[9] = 0x00      #直接返回值是字节得话


    # 转回不可变的 bytes 类型
    new_command = bytes(cmd_array)

    return new_command[0:32]



def modify_error_execution(command):
    # 修改指令的执行状态
    # 将字符串按空格分割成列表
    command_list = command.split()

    command_list[9] = '01'  # 参数错误
    # 截取列表长度
    new_command_list = command_list[:18]

    # 将处理后的列表元素用空格连接成字符串
    new_command = ' '.join(new_command_list)

    return new_command







# import struct

# lat=29
# lon=100
# alt=100
#
# lat=30.465
# lon=100.046
# alt=180
#
#
# little_endian_bytes = struct.pack('<d', lat)
# str=bytes_2_str_bytes(little_endian_bytes)
# print(str)
#
# lonlittle_endian_bytes = struct.pack('<d', lon)
# lonstr=bytes_2_str_bytes(lonlittle_endian_bytes)
# print(lonstr)
#
# altlittle_endian_bytes = struct.pack('<f', alt)
# altstr=bytes_2_str_bytes(altlittle_endian_bytes)
# print(altstr)


import pyproj
import math

# 定义地球半径（单位：米）
EARTH_RADIUS = 6371000


def calculate_bearing_and_pitch(lat1, lon1, h1, lat2, lon2, h2):
    # 创建转换器，将经纬度坐标转换为 ECEF（地心地固）笛卡尔坐标
    transformer = pyproj.Transformer.from_crs("EPSG:4326", "EPSG:4978")
    # 转换起点坐标
    x1, y1, z1 = transformer.transform(lat1, lon1, h1)
    # 转换终点坐标
    x2, y2, z2 = transformer.transform(lat2, lon2, h2)

    # 计算两点间的差值
    dx = x2 - x1
    dy = y2 - y1
    dz = z2 - z1

    # 计算方位角
    bearing = math.atan2(dy, dx)
    bearing = math.degrees(bearing)
    if bearing < 0:
        bearing += 360

    # 计算两点间的水平距离
    horizontal_distance = math.sqrt(dx ** 2 + dy ** 2)
    # 计算俯仰角
    pitch = math.atan2(dz, horizontal_distance)
    pitch = math.degrees(pitch)

    return bearing, pitch


def BLH2ECEF(lat, lon, h):
    transformer = pyproj.Transformer.from_crs("EPSG:4326", "EPSG:4978")
    return transformer.transform(lat, lon, h)

def ECEFToENU(refLat, refLon, refAlt, targetX, targetY, targetZ) :
    # // 将参考点转换为 ECEF 坐标
    refX, refY, refZ=BLH2ECEF(refLat, refLon, refAlt)

    # // 计算相对坐标
    dx = targetX - refX
    dy = targetY - refY
    dz = targetZ - refZ

    # // 计算旋转矩阵
    latRad = math.radians(refLat)
    lonRad = math.radians(refLon)

    eastMatrix = -math.sin(lonRad) * dx + math.cos(lonRad) * dy
    northMatrix = -math.sin(latRad) * math.cos(lonRad) * dx - math.sin(latRad) * math.sin(lonRad) * dy + math.cos(latRad) * dz
    up = math.cos(latRad) * math.cos(lonRad) * dx + math.cos(latRad) * math.sin(lonRad) * dy + math.sin(latRad) * dz

    east = eastMatrix
    north = northMatrix
    return east,north,up

def calculateAnglesFromENU(east, north, up) :
    azimuth = math.degrees(math.atan2(east, north))
    if azimuth < 0:
        azimuth += 360
    distance = math.sqrt(east * east + north * north)
    pitch = math.degrees(math.atan2(up, distance))
    return azimuth, math.fabs(pitch)


def calculate_azimuth_and_pitch(lat_radar, lon_radar, h__radar, lat_target, lon_target, h_target):

    targetX, targetY, targetZ = BLH2ECEF(lat_target, lon_target, h_target)
    east, north, up = ECEFToENU(lat_radar, lon_radar, h__radar, targetX, targetY, targetZ)
    return calculateAnglesFromENU(east, north, up)

def str_bytes_2_bytes(command):
    """
    将指令从明码编译为十六进制字节表示。

    :param command: 接收的bytes数据
    :return: 处理后的字节对象
    """
    command_hex = command.replace(" ", "")
    try:
        # 将十六进制字符串转换回字节对象
        command_bytes = bytes.fromhex(command_hex)
    except:
        command_bytes = bytes.fromhex('ffffff')

    return command_bytes